# 仿真器入口 用于训练强化学习 跟踪算法用的简化版本仿真器
# 由于没有GUI界面 
# 航控需要通过 位置偏差 和 航向偏差 等指标查看控制算法是否有效 
from dynamics_simulation import motionSim
import time
import pandas
import numpy as np
import random
import matplotlib.pyplot as plt
import socket
import json
from navigation import gpsAndUtm
from collections import namedtuple, OrderedDict
import traceback
from algo_struct.algo import rad_limit

# 解析控制参数
def decode_data(data):
    # load json
    recv_obser = json.loads(data)
    # print(recv_obser)
    delta = recv_obser['rudl']
    rsp = recv_obser['rspl']

    return delta, rsp

# 打包船的状态返回
def encode_data(trans):
    # 按照标准接口编写
    data = json.dumps(trans, sort_keys=True, indent=4, separators=(',', ':'))
    return data.encode('utf-8')

# 计算奖励
def get_reward(delta_psi, dpsi, delta):
    # 高频大舵角 得分高 但是这是执行机构做不到的 因此增加大舵角惩罚
    # score = -( 0.1*((delta_psi)**2) + (dpsi**2) + (delta**2))
    score = -( 0.1*((delta_psi)**2) + dpsi**2 + delta**2)

    print(f'score is {score}')
    return score

# test motion sim
if __name__ == '__main__':
    boat_data = dict()
    # 与航行控制软件连接TCP
    LOCAL_HOST = "127.0.0.1"
    LOCAL_PORT = 10905
    
    socket_server = socket.socket(family=socket.AF_INET, type=socket.SOCK_STREAM)
    socket_server.bind((LOCAL_HOST, LOCAL_PORT))
    socket_server.listen()
    conn, address = socket_server.accept()
    print(f"收到了客户端的连接，客户端信息是 {address}")

    all_sim_time = 200
    sim_time = 0
    delta_h = 0.1
    
    # 非线性kt参数
    K=3.1429
    T=18.348
    deltam=6.8207e-2
    alpha=6.8595e-3
    
    # boat运动仿真类输入参数
    my_mmg = motionSim.BoatMotionSim(delta_h=delta_h, K=K, T=T, deltam=deltam, alpha=alpha)
    
    delta = 0
    try:
        exp_angle = 90
        x = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])# 初始无人艇位置
        u = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])# 初始无人艇速度

        while True:
            sim_time += delta_h
            # 接收转速和舵角
            info, address = conn.recvfrom(1024)
            # print(f' recv tcp client data: {info}')
            delta, rsp = decode_data(info)
            # print(f'delta is {delta}, rsp is {rsp}')

            # 计算更新 位置 速度
            x, u, _ = my_mmg.runKt(x=x, u=u, delta=delta, f=rsp)
            
            # 为强化学习用
            observation=dict()
            observation['psi'] = rad_limit( (x[5] - exp_angle)*np.pi/180.0 )*180.0/np.pi
            observation['dpsi'] = u[5]
            observation['rud'] = delta/30.0 # 是否要约束到-1~1？ 效果是否有提升？
            boat_data['reward'] = get_reward(observation['psi'], observation['dpsi'], observation['rud'])

            if sim_time > all_sim_time:
                exp_angle = random.randint(-180, 180)
                x = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])# 初始无人艇位置
                u = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])# 初始无人艇速度

                rsp = 0
                delta = 0
                sim_time = 0
                done = 1
            else:
                done = 0

            boat_data['observation'] = observation
            boat_data['terminated'] = done
            boat_data['truncated'] = done
            data = encode_data(trans=boat_data)
            # 发送数据
            conn.send(data)
            
    except Exception:
        # 关闭连接
        conn.close()
        socket_server.close()
        traceback.print_exc()
